﻿#region Include
using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
#endregion
namespace SakerSomFaller
{
    class Physics
    {
        #region deklarationer
        public float h;
        Vector3 gravity;
        public Matrix3 angularVelocityStar;
        public const int NOTHINGTOWORRYABOUT = 0;
        public const int COLLISION = 1;
        public const int INTERPENETRATION = 2;
        public int collisionState = NOTHINGTOWORRYABOUT;
        public float time = 0;
        public  float CONTACTTHRESHOLD = 0.05f;
        public RigidBody[] rigidBodyList;
        public Couple[] coupleList;
        public List<Contact> contactList;
        #endregion

        #region initiering
        public void initPhysics(ref RigidBody rigBody, Vector3 forcePush, Vector3 pushPoint)
        {
            rigBody.density = 0.001f;
            h = 0.05f;

            contactList = new List<Contact>();

            angularVelocityStar = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);
            if(rigBody.rotation == null)
                rigBody.rotation = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

            //gravity = new Vector3(0f, -9.82f, 0f);
            gravity = new Vector3(0f, -2f, 0f);
            rigBody.oldRotation = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);
            rigBody.oldPosition = rigBody.position;

            rigBody.totalForce = gravity + forcePush;
            //rigBody.linearMomentum = h * forcePush;
            rigBody.linearMomentum = new Vector3();
            rigBody.angularMomentum = h * Vector3.Cross(pushPoint, forcePush);
            //Console.WriteLine("LinearMomentum: " + rigBody.linearMomentum);
            //Console.WriteLine("AngularMomentum: " + rigBody.angularMomentum);
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                calculateCentroid(ref rigBody.polygonArray[i]);
                calculatePolygonMass(ref rigBody.polygonArray[i], rigBody.density);
            }

            calculateObjectMass(ref rigBody);
            calculateInertiaTensorInverse(ref rigBody);
            calculateObjectCenterOfMass(ref rigBody);
            calculateNormals(ref rigBody);
        }
        #endregion
        #region ODE-lösare
        public void updatePhysics(ref RigidBody rigBody, float timestep)
        {
            h = timestep;

            if (collisionState == NOTHINGTOWORRYABOUT)
            {
                rigBody.oldPosition = rigBody.position;
                rigBody.oldAngularMomentum = rigBody.angularMomentum;
                rigBody.oldLinearMomentum = rigBody.linearMomentum;
                rigBody.oldRotation = rigBody.rotation;

                calculateMomentum(ref rigBody);
                calculatePosition(ref rigBody);
                calculateRotation(ref rigBody);

            }
            else if (collisionState == COLLISION)
            {
                /* calculateMomentum(ref rigBody);
                 calculatePosition(ref rigBody);
                 calculateRotation(ref rigBody);

                 rigBody.oldPosition = rigBody.position;
                 rigBody.oldAngularMomentum = rigBody.angularMomentum;
                 rigBody.oldLinearMomentum = rigBody.linearMomentum;
                 rigBody.oldRotation = rigBody.rotation;*/
            }
            time += h;
        }

        public void stepBack(ref RigidBody rigBody)
        {
            rigBody.position = rigBody.oldPosition;
            rigBody.angularMomentum = rigBody.oldAngularMomentum;
            rigBody.linearMomentum = rigBody.oldLinearMomentum;
            rigBody.rotation = rigBody.oldRotation;
        }
        #endregion

        #region local to world
        public VertexPositionColor[] localToWorldVPC(ref RigidBody rigBody, ref Polygon poly)
        {
            VertexPositionColor[] world = new VertexPositionColor[3];

            world[0].Position = rigBody.oldRotation * poly.vertices[0].Position + rigBody.oldPosition;
            world[1].Position = rigBody.oldRotation * poly.vertices[1].Position + rigBody.oldPosition;
            world[2].Position = rigBody.oldRotation * poly.vertices[2].Position + rigBody.oldPosition;
            return world;
        }

        public Vector3[] localToWorldV3(ref RigidBody rigBody, ref Polygon poly)
        {
            Vector3[] world = new Vector3[3];

            world[0] = rigBody.rotation * poly.vertices[0].Position + rigBody.position;
            world[1] = rigBody.rotation * poly.vertices[1].Position + rigBody.position;
            world[2] = rigBody.rotation * poly.vertices[2].Position + rigBody.position;
            return world;
        }
        #endregion

        #region initiala uträkningar
        public void calculateCentroid(ref Polygon triangle)
        {
            Vector3 centroid = new Vector3();

            for (int i = 0; i < triangle.vertices.Length; i++)
            {
                centroid.X += triangle.vertices[i].Position.X;
                centroid.Y += triangle.vertices[i].Position.Y;
                centroid.Z += triangle.vertices[i].Position.Z;
            }

            centroid.X = centroid.X / triangle.vertices.Length;
            centroid.Y = centroid.Y / triangle.vertices.Length;
            centroid.Z = centroid.Z / triangle.vertices.Length;

            triangle.centerOfMass = centroid;
            //System.Diagnostics.Debug.WriteLine("Centroid: " + centroid.X + ", " + centroid.Y + ", " + centroid.Z);
        }

        public void calculatePolygonMass(ref Polygon triangle, float density)
        {
            Vector3 AB = new Vector3();
            Vector3 AC = new Vector3();

            AB = Vector3.Subtract(triangle.vertices[0].Position, triangle.vertices[1].Position);
            AC = Vector3.Subtract(triangle.vertices[0].Position, triangle.vertices[2].Position);

            triangle.area = (float)(0.5 * (Vector3.Cross(AB, AC).Length()));
            triangle.mass = triangle.area * density;
        }

        public void calculateObjectMass(ref RigidBody rigBody)
        {
            float totalMass = 0.0f;

            for (int i = 0; i < rigBody.polygonArray.Count(); i++)
            {
                totalMass += rigBody.polygonArray[i].mass;
            }
            rigBody.totalMass = totalMass;
            //Console.WriteLine("total mass: " + totalMass);
        }

        public void calculateObjectCenterOfMass(ref RigidBody rigBody)
        {
            Vector3 COM = new Vector3();

            for (int i = 0; i < rigBody.polygonArray.Length; i++)
            {
                COM += rigBody.polygonArray[i].centerOfMass * rigBody.polygonArray[i].mass;
            }

            COM = COM / rigBody.totalMass;
            rigBody.centerOfMass = COM;
            //Console.WriteLine("COM " + COM);

            //COM i origo
            for (int i = 0; i < rigBody.polygonArray.Length; i++)
            {
                rigBody.polygonArray[i].vertices[0].Position = rigBody.polygonArray[i].vertices[0].Position - COM;
                rigBody.polygonArray[i].vertices[1].Position = rigBody.polygonArray[i].vertices[1].Position - COM;
                rigBody.polygonArray[i].vertices[2].Position = rigBody.polygonArray[i].vertices[2].Position - COM;
            }

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                calculateCentroid(ref rigBody.polygonArray[i]);

            }
        }

        public void calculateInertiaTensorInverse(ref RigidBody rigBody)
        {
            if (rigBody.stationary)
            {
                rigBody.inertiaTensorInverse = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0);
            }
            else
            {
                Matrix3 Ibody = new Matrix3();
                Ibody.M11 = rigBody.totalMass * ((float)Math.Pow(rigBody.height, 2.0) + (float)Math.Pow(rigBody.depth, 2.0)) / 12;
                Ibody.M22 = rigBody.totalMass * ((float)Math.Pow(rigBody.width, 2.0) + (float)Math.Pow(rigBody.depth, 2.0)) / 12;
                Ibody.M33 = rigBody.totalMass * ((float)Math.Pow(rigBody.height, 2.0) + (float)Math.Pow(rigBody.width, 2.0)) / 12;

                rigBody.inertiaTensorInverse = Ibody.Inverse();
            }
        }
        public Vector3 calculateCentroidForce(Polygon poly)
        {
            return (Vector3)(gravity);
        }

        public void calculateNormals(ref RigidBody rigBody)
        {
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {

                Vector3 a = rigBody.polygonArray[i].vertices[2].Position - rigBody.polygonArray[i].vertices[1].Position;
                Vector3 b = rigBody.polygonArray[i].vertices[1].Position - rigBody.polygonArray[i].vertices[0].Position;

                rigBody.polygonArray[i].normal = Vector3.Cross(b, a);
                rigBody.polygonArray[i].normal.Normalize();
            }
        }
        #endregion

        #region uträkningar till ODE-lösaren
        public void calculatePosition(ref RigidBody rigBody)
        {
            rigBody.position = rigBody.oldPosition + h * rigBody.linearMomentum / rigBody.totalMass;
        }

        public void calculateRotation(ref RigidBody rigBody)
        {
            rigBody.angularVelocity = rigBody.rotation * rigBody.inertiaTensorInverse * rigBody.rotation.Transverse() * rigBody.angularMomentum;

            angularVelocityStar.Star(rigBody.angularVelocity);
            rigBody.rotation = rigBody.oldRotation + h * angularVelocityStar * rigBody.rotation;
            rigBody.rotation = rigBody.rotation.GramSchmidt();
            //rigBody.angularVelocity = angularVelocity;
            //Console.WriteLine("Rotation: " + angularVelocity);
        }

        public void calculateMomentum(ref RigidBody rigBody)
        {
            Vector3 torque = new Vector3(0f, 0f, 0f);
            rigBody.totalForce = new Vector3();
            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                torque += Vector3.Cross(rigBody.oldRotation * rigBody.polygonArray[i].centerOfMass, gravity*rigBody.polygonArray[i].mass);
                //Console.WriteLine("torque: " + torque);
            }
            rigBody.totalForce = rigBody.totalMass*gravity + rigBody.normalForce;
            rigBody.torque = torque;
            rigBody.linearMomentum = rigBody.oldLinearMomentum + h * rigBody.totalForce;
            rigBody.angularMomentum = rigBody.oldAngularMomentum + h * torque;
            rigBody.normalForce = new Vector3();
            //Console.WriteLine("Linear Momentum: " + rigBody.linearMomentum);
            //Console.WriteLine("Angular Momentum: " + rigBody.angularMomentum);
        }
        // räknar ut polygonen triangles center of mass.
        #endregion


        #region kollisionsstuff
        public void findContactPoints(ref Couple couple)
        {
            RigidBody rigBodyA = rigidBodyList[couple.rigBodyA];
            RigidBody rigBodyB = rigidBodyList[couple.rigBodyB];
            RigidBody rigBody;
            contactList = new List<Contact>();
            
            if (rigBodyA.stationary)
            {
                for (int j = 0; j < rigBodyB.numberOfPolygons; j++)
                {
                    Vector3[] world = localToWorldV3(ref rigBodyB, ref rigBodyB.polygonArray[j]);
                    for (int k = 0; k < 3; k++)
                    {
                        if ((world[k].Y) <= -2.0f + CONTACTTHRESHOLD )
                        {
                            Boolean containsContact = new Boolean();
                            foreach (Contact contact in contactList)
                            {
                                if (contact.contactPoint == world[k])
                                    containsContact = true;
                            }
                            if ((world[k].Y) <= -2.0f + CONTACTTHRESHOLD && !containsContact)
                            {
                                Contact contact = new Contact();
                                contact.contactPoint = world[k];
                                contact.rigBodyA = couple.rigBodyA;
                                contact.rigBodyB = couple.rigBodyB;
                                contact.normal = new Vector3(0,1,0);
                                contactList.Add(contact);
                            }
                        }
                    }
                }
            }
            else //inte krock med mark
            {
                Vector3 normal, A;
                if (couple.faceOnA)
                {
                    normal = rigBodyA.rotation * couple.definingFace.normal;
                    normal.Normalize();
                    A = rigBodyA.rotation * couple.definingFace.vertices[0].Position + rigBodyA.position;
                    rigBody = rigBodyB;
                }
                else
                {
                    normal = rigBodyB.rotation * couple.definingFace.normal;
                    normal.Normalize();
                    A = rigBodyB.rotation * couple.definingFace.vertices[0].Position + rigBodyB.position;
                    rigBody = rigBodyA;
                }

                for (int i = 0; i < rigBody.numberOfPolygons; i++)
                {
                    for (int j = 0; j < 3; j++)
                    {
                        //A er en punkt i planet som testas, B är punkten man testar imot 
                        Vector3 B = rigBody.rotation * rigBody.polygonArray[i].vertices[j].Position + rigBody.position;
                        Vector3 AB = B - A;
                        Boolean containsContact = new Boolean();
                        foreach (Contact contact in contactList)
                        {
                            if (contact.contactPoint == B)
                                containsContact = true;
                        }
                        float test = Math.Abs(Vector3.Dot(AB, normal));
                        if (Math.Abs(Vector3.Dot(AB, normal)) < CONTACTTHRESHOLD && !containsContact)
                        {
                            Contact contact = new Contact();
                            contact.contactPoint = B;
                            contact.rigBodyA = couple.rigBodyA;
                            contact.rigBodyB = couple.rigBodyB;
                            contact.normal = normal;
                            contactList.Add(contact);
                        }
                    }
                }

            }
        }
    
        public int collisionType(ref Contact contact)
        {
            float vRel = Vector3.Dot(calculateRelativeVelocity(contact), contact.normal);
            if(vRel > Contact.COLLIDINGTHRESHOLD)
            {
                return Contact.COLLIDING;
            }
            if (vRel > -Contact.COLLIDINGTHRESHOLD)
            {
                return Contact.RESTING;
            }
            else
            {
                return Contact.COLLIDING;;
            }
        }
        public List<Contact> interpolateContacts(List<Contact> cl) {
            List<Contact> tempList = new List<Contact>();
            for (int i = 0; i < cl.Count(); i++)
            {
                    Contact temp = new Contact();
                    temp.rigBodyA = cl[i].rigBodyA;
                    temp.rigBodyB = cl[i].rigBodyB;
                    temp.normal = cl[i].normal;
                    Vector3 contactPointSum = new Vector3();
                    int nrContacts = 0;
                    for (int j = 0; j < cl.Count(); j++)
                    {
                        if (cl[i].rigBodyA == cl[j].rigBodyA && cl[i].rigBodyB == cl[j].rigBodyB)
                        {
                            contactPointSum += cl[j].contactPoint;
                            nrContacts++;

                        }
                    }
                    temp.contactPoint = contactPointSum / nrContacts;
                    bool containsContact = false;
                    foreach (Contact contact in tempList)
                    {
                        if (contact.contactPoint == temp.contactPoint)
                            containsContact = true;
                    }

                    if (!containsContact)
                        tempList.Add(temp);
            }
            return tempList;
        }


        public Vector3 calculateRelativeVelocity(Contact contact)
        {
            RigidBody rigBodyA = rigidBodyList[contact.rigBodyA];
            RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];

            Vector3 velocityA = rigBodyA.linearMomentum / rigBodyA.totalMass;
            Vector3 velocityB = rigBodyB.linearMomentum / rigBodyB.totalMass;

            Vector3 pointVelocityA = velocityA + Vector3.Cross(rigBodyA.angularVelocity, (contact.contactPoint - rigBodyA.position));
            Vector3 pointVelocityB = velocityB + Vector3.Cross(rigBodyB.angularVelocity, (contact.contactPoint - rigBodyB.position));

            return pointVelocityA - pointVelocityB;
        }
        public void resolveCollision(ref List<Contact> contactList)
        {
            foreach (Contact c in contactList)
            {
                Contact contact = c;
                collisionType(ref contact);
                if(collisionType(ref contact) == Contact.COLLIDING)
                {
                    collidingContact(ref contact);
                }
                else if (collisionType(ref contact) == Contact.RESTING)
                {
                    restingContact(ref contact);
                }
                else
                {
                  RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];
                  rigBodyB.normalForce = new Vector3();
                }
            }
            collisionState = NOTHINGTOWORRYABOUT;
        }
        public void collidingContact(ref Contact contact)
        {
                RigidBody rigBodyA = rigidBodyList[contact.rigBodyA];
                RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];

                    float elasticity = 0.0f;
                    float Ma = rigBodyA.totalMass;
                    float Mb = rigBodyB.totalMass;
                    Matrix3 invIA = rigBodyA.inertiaTensorInverse;
                    Matrix3 invIB = rigBodyB.inertiaTensorInverse;


                    Vector3 n = contact.normal;
                    double vRel = Vector3.Dot(n, calculateRelativeVelocity(contact));
                    Vector3 rA = contact.contactPoint;
                    Vector3 rB = contact.contactPoint - rigBodyB.position;

                    double numerator = -(1 + elasticity) * vRel;
                    double term1 = (rigBodyA.stationary) ? 0 : 1 / Ma;
                    double term2 = (rigBodyB.stationary) ? 0 : 1 / Mb;
                    double term3 = Vector3.Dot(n, (Vector3.Cross((invIA * Vector3.Cross(rA, n)), rA)));
                    double term4 = Vector3.Dot(n, (Vector3.Cross((invIB * Vector3.Cross(rB, n)), rB)));
                    double j = numerator / (term1 + term2 + term3 + term4);
                    Vector3 J = (float)j * n;

                    Vector3 torqueImpulse = new Vector3();
                    if (rigBodyA.stationary)
                    {
                         torqueImpulse = Vector3.Cross(rB, J);
                    }
                    else
                    {
                         torqueImpulse = Vector3.Cross(rA, J);
                    }

                    if (!rigBodyA.stationary)
                    {

                        rigBodyA.linearMomentum += J;
                        rigBodyA.angularMomentum += torqueImpulse;
                    }
                    rigBodyB.linearMomentum -= J;
                    rigBodyB.angularMomentum -= torqueImpulse;

                    rigidBodyList[contact.rigBodyA] = rigBodyA;
                    rigidBodyList[contact.rigBodyB] = rigBodyB;
        }
       
        public float calculateSeparatingVelocity(Contact contact)
        {
            Vector3 n = contact.normal;
            Vector3 relV = calculateRelativeVelocity(contact);

            return Vector3.Dot(n, relV);

        }
        
        public Vector3 calculateRelativeAcceleration(Contact contact)
        {
            RigidBody rigBodyA = rigidBodyList[contact.rigBodyA];
            RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];

            Vector3 n = contact.normal;
            Vector3 relV = calculateRelativeVelocity(contact);
            Matrix3 invIA = rigBodyA.inertiaTensorInverse;
            Matrix3 invIB = rigBodyB.inertiaTensorInverse;
            Vector3 LA = rigBodyA.angularMomentum;
            Vector3 LB = rigBodyB.angularMomentum;
            Vector3 torqueA = rigBodyA.torque;
            Vector3 torqueB = rigBodyA.torque;
            Vector3 vDotA = rigBodyA.totalForce / rigBodyA.totalMass;
            Vector3 vDotB = rigBodyB.totalForce / rigBodyB.totalMass;
            Vector3 accA = vDotA + invIA*torqueA + invIA*(Vector3.Cross(LA,torqueA));
            Vector3 accB = vDotB + invIA*torqueB + invIB*(Vector3.Cross(LA,torqueB));

            return accA - accB;

        }

        public float calculateSeparatingAcceleration(Contact contact)
        {
            Vector3 n = contact.normal;
            Vector3 dotN = calculateNormalDot(ref contact);
            Vector3 relA = calculateRelativeAcceleration(contact);
            Vector3 relV = calculateRelativeVelocity(contact);

            return Vector3.Dot(n, relA) + Vector3.Dot(2 * dotN, relV);
        }

        public Vector3 calculateNormalDot(ref Contact contact)
        {
            RigidBody rigBodyA = rigidBodyList[contact.rigBodyA];
            RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];
            Couple couple = coupleList[contact.rigBodyA];

            if (couple.faceOnA)
            {
                return Vector3.Cross(rigBodyA.angularVelocity,contact.normal);
            }
            else
            {
                return Vector3.Cross(rigBodyB.angularVelocity, contact.normal);
            }

        }


        public void restingContact(ref Contact contact)
        {
            RigidBody rigBodyB = rigidBodyList[contact.rigBodyB];

                float separatingAcceleration = calculateSeparatingAcceleration(contact);
                if (separatingAcceleration > 0)
                {
                    rigBodyB.normalForce = -separatingAcceleration * contact.normal * rigBodyB.totalMass;
                }

                rigidBodyList[contact.rigBodyB] = rigBodyB;


             

        }

        #endregion

        #region separationsplan
       /* public Boolean checkSeparatingPlane(ref Couple couple)
        {
            int rightSideCheck = 0;
            RigidBody rigBody;
            Vector3 normal, A;
            if (couple.faceOnA)
            {
                normal = couple.rigBodyA.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyA.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyA.position;
                rigBody = couple.rigBodyB;
            }
            else
            {
                normal = couple.rigBodyB.rotation * couple.definingFace.normal;
                normal.Normalize();
                A = couple.rigBodyB.rotation * couple.definingFace.vertices[0].Position + couple.rigBodyB.position;
                rigBody = couple.rigBodyA;
            }

            for (int i = 0; i < rigBody.numberOfPolygons; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    //A er en punkt i planet som testas, B är punkten man testar imot 
                    Vector3 B = rigBody.polygonArray[i].vertices[j].Position + rigBody.position;
                    Vector3 AB = B - A;
                    if (Vector3.Dot(AB, normal) > 0)// no collision
                    {
                        rightSideCheck++;
                    }
                }
            }
            if (rightSideCheck == rigBody.numberOfPolygons * 3) //antalet vertexar i hela rigbodyn //separationsplan!!!
            {
                //collisionState = NOTHINGTOWORRYABOUT;
                return true;
            }
            collisionState = COLLISION;
            return false;
        }*/

        public Boolean calculateSeparatingPlane(ref Couple couple)
        {
            RigidBody rigBodyA = rigidBodyList[couple.rigBodyA];
            RigidBody rigBodyB = rigidBodyList[couple.rigBodyB];

            if (rigBodyA.stationary)
            {
                for (int j = 0; j < rigBodyB.numberOfPolygons; j++)
                {
                    Vector3[] world = localToWorldV3(ref rigBodyB, ref rigBodyB.polygonArray[j]);
                    for (int k = 0; k < 3; k++)
                    {
                        if (world[k].Y  <= -2.5f + CONTACTTHRESHOLD)
                        {
                            return false;
                        }
                    }
                }
                couple.faceOnA = true;
                couple.planeExists = true;
                couple.definingFace = rigBodyA.polygonArray[2];
                return true;
            }

            for (int i = 0; i < rigBodyA.numberOfPolygons; i++)
            {
                Vector3 normal = rigBodyA.rotation * rigBodyA.polygonArray[i].normal;
                normal.Normalize();
                int rightSideCheck = 0;
                for (int j = 0; j < rigBodyB.numberOfPolygons; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        //A er en punkt i planet som testas, B är punkten man testar imot 
                        Vector3 A = rigBodyA.rotation * rigBodyA.polygonArray[i].vertices[k].Position + rigBodyA.position;
                        Vector3 B = rigBodyB.rotation * rigBodyB.polygonArray[j].vertices[k].Position + rigBodyB.position;
                        Vector3 AB = B - A;
                        if (Vector3.Dot(AB, normal) >= 0)
                        { // no collision
                            rightSideCheck++;
                        }
                    }
                }
                if (rightSideCheck == rigBodyB.numberOfPolygons * 3)
                { //separationsplan!!!
                    couple.faceOnA = true;
                    couple.planeExists = true;
                    couple.sepIndex = i;
                    couple.definingFace = rigBodyA.polygonArray[i];
                    return true;
                }
            }

            for (int i = 0; i < rigBodyB.numberOfPolygons; i++)
            {
                Vector3 normal = rigBodyB.rotation * rigBodyB.polygonArray[i].normal;
                normal.Normalize();
                int rightSideCheck = 0;
                Vector3 A = rigBodyB.rotation * rigBodyB.polygonArray[i].vertices[0].Position + rigBodyB.position;
                for (int j = 0; j < rigBodyA.numberOfPolygons; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        //A er en punkt i planet som testas, B är punkten man testar imot                       
                        Vector3 B = rigBodyA.rotation * rigBodyA.polygonArray[j].vertices[k].Position + rigBodyA.position;
                        Vector3 AB = B - A;
                        if (Vector3.Dot(AB, normal) >= 0)
                        { // no collision
                            rightSideCheck++;
                        }
                    }
                }
                if (rightSideCheck == rigBodyA.numberOfPolygons * 3)
                { //separationsplan!!!
                    couple.faceOnA = false;
                    couple.planeExists = true;
                    couple.sepIndex = i;
                    couple.definingFace = rigBodyB.polygonArray[i];
                    return true;
                }
            }
            collisionState = COLLISION;
            return false;
        }
        #endregion
         
    }
}




